#pragma once

#include "SLAM/common.hpp"

namespace SLAM {

// SVD 点到点对齐（2D）
inline void svd_align_2d(const std::vector<Eigen::Vector2d>& A,
                         const std::vector<Eigen::Vector2d>& B,
                         double& x, double& y, double& yaw)
{
  const size_t N = A.size();
  Eigen::Vector2d ca(0,0), cb(0,0);
  for (size_t i=0;i<N;++i){ ca+=A[i]; cb+=B[i]; }
  ca/=N; cb/=N;

  Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
  for (size_t i=0;i<N;++i){
    Eigen::Vector2d da = A[i]-ca, db=B[i]-cb;
    H += da*db.transpose();
  }
  Eigen::JacobiSVD<Eigen::Matrix2d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix2d R = svd.matrixV() * svd.matrixU().transpose();
  if (R.determinant()<0) {
    Eigen::Matrix2d V = svd.matrixV(); V(0,1) *= -1; V(1,1) *= -1;
    R = V * svd.matrixU().transpose();
  }
  yaw = std::atan2(R(1,0), R(0,0));
  Eigen::Vector2d t = cb - R*ca;
  x = t.x(); y = t.y();
}

// 用内点集合做少量迭代的 point-to-point ICP（2D）
inline void icp_refine_se2(const std::vector<Reflector>& obs_base,
                           const std::vector<Landmark>&  lms_map,
                           const std::vector<MatchPair>& matches,
                           const std::vector<int>& inlier_idx,
                           double& x, double& y, double& yaw,
                           int iters=5)
{
  for (int it=0; it<iters; ++it){
    std::vector<Eigen::Vector2d> A, B;
    const double c=std::cos(yaw), s=std::sin(yaw);
    for (int k : inlier_idx){
      const auto &mp = matches[k];
      // A: 变换后的 base 点；B: 对应 lm
      Eigen::Vector2d a( obs_base[mp.obs_index].x, obs_base[mp.obs_index].y );
      Eigen::Vector2d Ra(c*a.x()-s*a.y()+x, s*a.x()+c*a.y()+y);
      A.push_back(Ra);
      for (auto &L : lms_map) if (L.id==mp.lm_index) { B.emplace_back(L.x, L.y); break; }
    }
    if (A.size()<3) return;

    double nx=x, ny=y, nyaw=yaw;
    svd_align_2d(A, B, nx, ny, nyaw);
    x=nx; y=ny; yaw=nyaw;
  }
}

} // namespace SLAM
